/*

iod.pde -> Input, Output, Devices

ArduPilotMegaJeti Version 1.0 Public Beta
Author:     Uwe Gartmann

Authors of ArduPilotMega:	Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Michael Smith
Thanks to:	Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi

This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
/// Definitions

/// Includes

/// Declarations

void initIOD(void) {
// Init hardware & sensors
	IO.Init();
	Serial0.begin(SERIAL0_BAUD);
	
// IMU
	IMU.Init();
	readIMUOffsets();
	IMU.Scale[IMU_pitch] = 0.4;
	IMU.Scale[IMU_roll]  = 0.4;
	IMU.Scale[IMU_yaw]   = 0.4;
	
// Altimeter
	Altimeter.Init();
	
// Compass
	Compass.SetOrientation(MAGNETOMETER_ORIENTATION);
	Compass.Init();
	readCompassMaxMinValues();

// reverse switches
	read_reverse_switches();	// read configuration parameters

// Others
	LT.Init();					// initialize loop timers
}

void updateIOD(void)  {
	Altimeter.Update();
	Compass.Update(0,0);
}

void updateIMU(void) {
	IMU.Update();
}

void saveIMUOffsets(void) {
	IMU.calibrateACC();
	IMU.calibrateGYRO();
	eeprom_write_float(EE_ACC_OFFSET_X, IMU.Offset[IMU_accX]);
	eeprom_write_float(EE_ACC_OFFSET_Y, IMU.Offset[IMU_accY]);
	eeprom_write_float(EE_ACC_OFFSET_Z, IMU.Offset[IMU_accZ]);
	eeprom_write_float(EE_GYRO_OFFSET_ROLL, IMU.Offset[IMU_roll]);
	eeprom_write_float(EE_GYRO_OFFSET_PITCH, IMU.Offset[IMU_pitch]);
	eeprom_write_float(EE_GYRO_OFFSET_YAW, IMU.Offset[IMU_yaw]);
}

void readIMUOffsets(void) {
	IMU.Offset[IMU_accX] = eeprom_read_float(EE_ACC_OFFSET_X);
	IMU.Offset[IMU_accY] = eeprom_read_float(EE_ACC_OFFSET_Y);
	IMU.Offset[IMU_accZ] = eeprom_read_float(EE_ACC_OFFSET_Z);
	IMU.Offset[IMU_roll] = eeprom_read_float(EE_GYRO_OFFSET_ROLL);
	IMU.Offset[IMU_pitch] = eeprom_read_float(EE_GYRO_OFFSET_PITCH);
	IMU.Offset[IMU_yaw] = eeprom_read_float(EE_GYRO_OFFSET_YAW);
}

// Compass
void saveCompassDeclination(void) {
	eeprom_write_float(EE_COMPASS_DECLINATION, Compass.declination);
}

void clearCompassMaxMinValue(void) {
	Compass.Mag_Xmax = Compass.Mag_Xmin = 0;
	Compass.Mag_Ymax = Compass.Mag_Ymin = 0;
	Compass.Mag_Zmax = Compass.Mag_Zmin = 0;
}

void saveCompassMaxMinValues(void) {
	eeprom_write_int(EE_COMPASS_X_MAX,Compass.Mag_Xmax);
	eeprom_write_int(EE_COMPASS_X_MIN,Compass.Mag_Xmin);
	eeprom_write_int(EE_COMPASS_Y_MAX,Compass.Mag_Ymax);
	eeprom_write_int(EE_COMPASS_Y_MIN,Compass.Mag_Ymin);
	eeprom_write_int(EE_COMPASS_Z_MAX,Compass.Mag_Zmax);
	eeprom_write_int(EE_COMPASS_Z_MIN,Compass.Mag_Zmin);
}

void readCompassMaxMinValues(void) {
	Compass.declination = eeprom_read_float(EE_COMPASS_DECLINATION);
	Compass.Mag_Xmax = eeprom_read_int(EE_COMPASS_X_MAX);
	Compass.Mag_Xmin = eeprom_read_int(EE_COMPASS_X_MIN);
	Compass.Mag_Ymax = eeprom_read_int(EE_COMPASS_Y_MAX);
	Compass.Mag_Ymin = eeprom_read_int(EE_COMPASS_Y_MIN);
	Compass.Mag_Zmax = eeprom_read_int(EE_COMPASS_Z_MAX);
	Compass.Mag_Zmin = eeprom_read_int(EE_COMPASS_Z_MIN);
}

// Serial
void serialFloatPrint(float f) {
  byte * b = (byte *) &f;
  Serial0.print("f:");
  for(int i=0; i<4; i++) {
    
    byte b1 = (b[i] >> 4) & 0x0f;
    byte b2 = (b[i] & 0x0f);
    
    char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
    char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
    
    Serial0.print(c1);
    Serial0.print(c2);
  }
}

unsigned long freeRAM() {
	uint8_t * heapptr, * stackptr;
	stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
	heapptr = stackptr; // save value of heap pointer
	free(stackptr); // free up the memory again (sets stackptr to 0)
	stackptr = (uint8_t *)(SP); // save value of stack pointer
	return stackptr - heapptr;
}



